
#ifndef __COMM_CAR_CPP__
#define __COMM_CAR_CPP__

#include "comm/comm_car.h"
#include "comm/comm_base.h"
#include <math.h>
#include <iostream>
#include <list>
#include <vector>

using namespace std;

LASER_C::LASER_C(void)
{
    scanAgnleRange_  = 190.0 / 180.0 * PI;
    scanPointNum_    = 381;
    pointAngleBase_  = -0.5 * scanAgnleRange_;
    pointAngleInc_   = scanAgnleRange_ / (float)(scanPointNum_ - 1);
    pointDensity_    = 1.0 / pointAngleInc_;

    blindObstDetectDistMin_             = SCAN_RANGE_FURTHER;
    blindObstDetectDistMax_             = SCAN_RANGE_FURTHER;
    minRightBlindPointIdx_              = 0;         /* -95 ~ -90度 */
    maxRightBlindPointIdx_              = 10;
    minLeftBlindPointIdx_               = 371;       /* 90 ~ 95度 */
    MaxLeftBlindPointIdx_               = 381;
    
    for (unsigned int loop = 0; loop < scanPointNum_; loop++) {
        float pointAngle = GetPointAngleByIdx(loop);
        laserPointCos_.push_back(cos(pointAngle));
        laserPointSin_.push_back(sin(pointAngle));
    }

    stPoseByMap_.SetZero();
}

LASER_C::~LASER_C(void)
{
}

void LASER_C::SetPos(COORDINATE_2D_STRU *pstLaserPos, float laserDirAngle, float dirAngleSin, float dirAngleCos)
{
    stPoseByMap_.stPos.x  = pstLaserPos->x;
    stPoseByMap_.stPos.y  = pstLaserPos->y;
    stPoseByMap_.dirAngle = laserDirAngle;

    objMapToLaserTf_.SetTransRelationship(stPoseByMap_.stPos, stPoseByMap_.dirAngle, dirAngleSin, dirAngleCos);
}

void LASER_C::SetPose(const COOR_2D_POSE_STRU &stPose)
{
    stPoseByMap_.Copy(stPose);

    objMapToLaserTf_.SetTransRelationship(stPose);
}

const COORDINATE_2D_STRU &LASER_C::GetPos(void) const
{
    return stPoseByMap_.stPos;
}

const COOR_2D_POSE_STRU &LASER_C::GetPoseByMap(void)
{
    return stPoseByMap_;
}

void LASER_C::GetPoseByMap(COOR_2D_POSE_STRU &stPose)
{
    stPose.Copy(stPoseByMap_);
}

/**************************************************************************************
功能描述: 将地图位置坐标转换为雷达坐标位置
修改记录:
**************************************************************************************/
void LASER_C::TransPosByMapToByLaser(const COORDINATE_2D_STRU &stPosByMap, COORDINATE_2D_STRU &stPosByLaser)
{
    COOR_2D_POSE_STRU stPoseByMap;
    stPoseByMap.dirAngle = 0.0;
    stPoseByMap.stPos.Copy(stPosByMap);

    COOR_2D_POSE_STRU stPoseByLaser;
    objMapToLaserTf_.TransFatherPoseToSon(stPoseByMap, stPoseByLaser);
    stPosByLaser.Copy(stPoseByLaser.stPos);
}

void LASER_C::InitLeftAvoidDist(float carMinTrunRadius, float laserXByCar, float protectXByCar, float protectYByCar)
{
    /* 涉及各种三角计算，无法用名词表达各个变量函数，需参照几何图形 */
    float angle1 = atan2(carMinTrunRadius, laserXByCar);
    float protecTurnRadius = hypot(carMinTrunRadius + protectYByCar, protectXByCar);
    float frontWheelTurnRadius = hypot(carMinTrunRadius, laserXByCar);

    for (unsigned int loop = 0; loop < scanPointNum_; loop++) {
        float pointAngle = GetPointAngleByIdx(loop);
        float angle2 = PI - angle1 - pointAngle;
        float minAvoidMinDist;
        if (ABS(angle2 - PI) < 0.01) {
            minAvoidMinDist = protecTurnRadius - frontWheelTurnRadius;
        } else {
            if (angle2 > PI) {
                angle2 = 2 * PI - angle2;
            }
            float angle3 = asin(sin(angle2) / protecTurnRadius * frontWheelTurnRadius);
            float angle4 = PI - angle2 - angle3;
            minAvoidMinDist = sin(angle4) * protecTurnRadius / sin(angle2);
        }

        if (pointAngle < 0) {
            float distToRightProtectLine = ABS(protectYByCar / sin(pointAngle));
            minAvoidMinDist = MIN(minAvoidMinDist, distToRightProtectLine);
        }

        leftAvoidMinDistPerPoint_.push_back(minAvoidMinDist);
        
        //ST_LOG_INFO("%f, %f, %d, %f, %f.", minAvoidMinDist, pointAngle, loop,
        //                   cos(pointAngle) * minAvoidMinDist, 
        //                   sin(pointAngle) * minAvoidMinDist);
    }
}

float LASER_C::GetLeftAvoidMinDist(unsigned int angleIdx)
{
    if (angleIdx >= scanPointNum_) {
        return 10000.0;
    }

    return leftAvoidMinDistPerPoint_[angleIdx];
}

float LASER_C::GetRightAvoidMinDist(unsigned int angleIdx)
{
    if (angleIdx >= scanPointNum_) {
        return 10000.0;
    }

    return leftAvoidMinDistPerPoint_[scanPointNum_ - angleIdx - 1];
}

float LASER_C::GetPointAngleByIdx(int pointIdx) const
{
    return pointAngleBase_ + pointAngleInc_ * pointIdx;
}

UINT32 LASER_C::GetPointAngleIdx(float angle) const
{
    if ((angle < pointAngleBase_) || (angle > (pointAngleBase_ + scanAgnleRange_))) {
        return UINT32_NULL;
    }

    return UINT32((angle - pointAngleBase_) * pointDensity_ + 0.5);
}

void LASER_C::GetPointPos(float pointAngle, float pointDist, COORDINATE_2D_STRU *pstPointPos) const
{
    float pointAngleByMap = AdjustAngle(stPoseByMap_.dirAngle + pointAngle);
    
    pstPointPos->x = stPoseByMap_.stPos.x + pointDist * cos(pointAngleByMap);
    pstPointPos->y = stPoseByMap_.stPos.y + pointDist * sin(pointAngleByMap);
}

void LASER_C::GetPointPosByMap(UINT32 pointIdx, float pointDist, COORDINATE_2D_STRU &stPointPosByMap) const
{
    COORDINATE_2D_STRU stPointPosByLaser;
    GetPointPosByLaser(pointIdx, pointDist, &stPointPosByLaser);

    objMapToLaserTf_.TransSonPosToFather(stPointPosByLaser, stPointPosByMap);
}

float LASER_C::GetPointAngleByPos(const COORDINATE_2D_STRU &stPointPos) const
{
    float pointAngleByMap = Calc2PointLineAngle(&stPoseByMap_.stPos, &stPointPos);
    return AdjustAngle(pointAngleByMap - stPoseByMap_.dirAngle);
}

float LASER_C::GetPointAngleByMapByIdx(int pointIdx) const
{
    float pointAngle = GetPointAngleByIdx(pointIdx);
    return AdjustAngle(stPoseByMap_.dirAngle + pointAngle);
}

void LASER_C::GetPointByPos(const COORDINATE_2D_STRU &stPointPos, LASER_POINT_STRU &stPoint) const
{
    stPoint.angle = GetPointAngleByPos(stPointPos);
    stPoint.dist  = sqrt(stPointPos.CalcDistSqu(stPoseByMap_.stPos));    
}

void LASER_C::GetPointPosByLaser(unsigned int angleIdx, float dist, COORDINATE_2D_STRU *pstRelDist) const
{
    if ((angleIdx < 0) || (angleIdx >= scanPointNum_) || (dist <= 0.0)) {
        pstRelDist->y  = 100.0;  /* 非法点设置为极大值无影响 */
        pstRelDist->x = 100.0;
        return;
    }
    
    pstRelDist->y = dist * laserPointSin_[angleIdx];
    pstRelDist->x = dist * laserPointCos_[angleIdx];
}

void LASER_C::GetPointPosByLaser(float angle, float dist, COORDINATE_2D_STRU *pstRelDist) const
{
    pstRelDist->y  = dist * sin(angle);
    pstRelDist->x = dist * cos(angle);
}

void LASER_C::SetBlindObstDetectDist(float distMin, float distMax)
{
    blindObstDetectDistMin_ = distMin;
    blindObstDetectDistMax_ = distMax;
}

bool LASER_C::GetRightBlindObst(const float aLaserPoint[], UINT32 laserPointNum, COORDINATE_2D_STRU *pstObstPos) const
{
    if (laserPointNum != scanPointNum_) {
        return false;
    }
    
    return GetBlindObst(aLaserPoint,
                        minRightBlindPointIdx_,
                        maxRightBlindPointIdx_,
                        pstObstPos);
}

bool LASER_C::GetLeftBlindObst(const float aLaserPoint[], UINT32 laserPointNum, COORDINATE_2D_STRU *pstObstPos) const
{
    if (laserPointNum != scanPointNum_) {
        return false;
    }

    return GetBlindObst(aLaserPoint,
                        minLeftBlindPointIdx_,
                        MaxLeftBlindPointIdx_,
                        pstObstPos);
}

bool LASER_C::GetBlindObst(const float aLaserPoint[], int minDetectIdx, int maxDetectIdx, COORDINATE_2D_STRU *pstObstPos) const
{   
    float minDist = blindObstDetectDistMax_;
    int   minDistIdx = 0;
    
    for (int loop = minDetectIdx; loop < maxDetectIdx; loop++) {
        float pointDist = aLaserPoint[loop];
        if (pointDist < blindObstDetectDistMin_) {
            continue;
        }
        
        if (pointDist < minDist) {
            minDist = pointDist;
            minDistIdx = loop;
        }
    }

    if (minDist < blindObstDetectDistMax_) {
        GetPointPos(GetPointAngleByIdx(minDistIdx), minDist, pstObstPos);
        return true;
    }

    return false;
}

/**************************************************************************************
功能描述: 获取车体左前点的位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetLeftHeadPosByCarWithProtect(float headProtectWidth, float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const
{
    stPosByCar.x = stLeftHeadPosByCar_.x + headProtectWidth;
    stPosByCar.y = stLeftHeadPosByCar_.y + sideProtectWidth;
}

/**************************************************************************************
功能描述: 获取车体右前点的位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetRightHeadPosByCarWithProtect(float headProtectWidth, float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const
{
    stPosByCar.x = stRightHeadPosByCar_.x + headProtectWidth;
    stPosByCar.y = stRightHeadPosByCar_.y - sideProtectWidth;
}

/**************************************************************************************
功能描述: 获取车体后轴左侧点位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetLeftBackAxisPosByCarWithProtect(float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const
{
    stPosByCar.x = stLeftBackAxisPosByCar_.x;
    stPosByCar.y = stLeftBackAxisPosByCar_.y + sideProtectWidth;
}

/**************************************************************************************
功能描述: 获取车体后轴右侧点位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetRightBackAxisPosByCarWithProtect(float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const
{
    stPosByCar.x = stRightBackAxisPosByCar_.x;
    stPosByCar.y = stRightBackAxisPosByCar_.y - sideProtectWidth;
}

/**************************************************************************************
功能描述: 获取车体左后点位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetLeftTailPosByCarWithProtect(float tailProtectWidth, float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const
{
    stPosByCar.x = stLeftTailPosByCar_.x - tailProtectWidth;
    stPosByCar.y = stLeftTailPosByCar_.y + sideProtectWidth;
}

/**************************************************************************************
功能描述: 获取车体右后点位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetRightTailPosByCarWithProtect(float tailProtectWidth, float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const
{
    stPosByCar.x = stRightTailPosByCar_.x - tailProtectWidth;
    stPosByCar.y = stRightTailPosByCar_.y - sideProtectWidth;
}

/**************************************************************************************
功能描述: 获取最小半径左转圆心位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetMaxAngleLeftTurnCenterPosByCar(COORDINATE_2D_STRU &stPosByCar) const
{
    stPosByCar = stMaxAngleLeftTurnCenterPosByCar_;
}

/**************************************************************************************
功能描述: 获取最小半径右转圆心位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetMaxAngleRightTurnCenterPosByCar(COORDINATE_2D_STRU &stPosByCar) const
{
    stPosByCar = stMaxAngleRightTurnCenterPosByCar_;
}

/**************************************************************************************
功能描述: 获取指定转向角时的转弯圆心位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetLeftTurnCenterPosByCar(float steeringAngleAbs, COORDINATE_2D_STRU &stPosByCar) const
{
    float steeringAngleAbsE = MIN(ABS(steeringAngleAbs), stBodyParam_.maxSteeringAngleAbs);
    float turnRadius        = GetTurnRadius(steeringAngleAbsE);
    
    stPosByCar.x  = 0.0;
    stPosByCar.y  = turnRadius;
}

/**************************************************************************************
功能描述: 获取指定转向角时的转弯圆心位置
修改记录:
**************************************************************************************/
void CAR_BODY_C::GetRightTurnCenterPosByCar(float steeringAngleAbs, COORDINATE_2D_STRU &stPosByCar) const
{
    float steeringAngleAbsE = MIN(ABS(steeringAngleAbs), stBodyParam_.maxSteeringAngleAbs);
    float turnRadius        = GetTurnRadius(steeringAngleAbsE);
    
    stPosByCar.x  = 0.0;
    stPosByCar.y  = turnRadius * -1.0;
}

/**************************************************************************************
功能描述: 获取前半身长度
修改记录:
**************************************************************************************/
float CAR_BODY_C::GetHeadHalfBodyLen(void) const
{
    return stBodyParam_.headToBackAxisLen;
}

/**************************************************************************************
功能描述: 获取后半身长度
修改记录:
**************************************************************************************/
float CAR_BODY_C::GetBackHalfBodyLen(void) const
{
    return stBodyParam_.tailToBackAxisLen;
}

/**************************************************************************************
功能描述: 获取前轮之前的车身长度
修改记录:
**************************************************************************************/
float CAR_BODY_C::GetBodyLenBeforeFrontWheel(void) const
{
    return stBodyParam_.headToBackAxisLen - stBodyParam_.wheelBaseLen;
}

/**************************************************************************************
功能描述: 获取车身宽度
修改记录:
**************************************************************************************/
float CAR_BODY_C::GetBodyWidth(void) const
{
    return stBodyParam_.bodyWidth;
}

/**************************************************************************************
功能描述: 获取最大转向角
修改记录:
**************************************************************************************/
float CAR_BODY_C::GetMaxSteeringAngleAbs(void) const
{
    return stBodyParam_.maxSteeringAngleAbs;
}

/**************************************************************************************
功能描述: 获取最大速度
修改记录:
**************************************************************************************/
float CAR_BODY_C::GetMaxMoveVelAbs(void) const
{
    return stBodyParam_.maxMoveVelAbs;
}

/**************************************************************************************
功能描述: 获取给定转向角下的转弯半径
修改记录:
**************************************************************************************/
float CAR_BODY_C::GetTurnRadius(float steeringAngle) const
{
    return (stBodyParam_.wheelBaseLen * tan(0.5 * PI - ABS(steeringAngle)));
}

/**************************************************************************************
功能描述: 获取给定转弯半径(指后轴中心处)下的转向角
          半径为正，圆心在左边，转向角大于0
          半径为负，圆心在右边，转向角小于0
修改记录:
**************************************************************************************/
float CAR_BODY_C::GetSteeringAngle(float turnRadius) const
{
    return SIGN(turnRadius) * atan2(stBodyParam_.wheelBaseLen, ABS(turnRadius));
}

INT32 CAR_BODY_C::ResetBody(CAR_BODY_PARAM_STRU &stBodyParam)
{
    stFrontWheelPoseByCar_.stPos.x  = stBodyParam.wheelBaseLen;
    stFrontWheelPoseByCar_.stPos.y  = 0.0;
    stFrontWheelPoseByCar_.dirAngle = 0.0;
        
    stLeftHeadPosByCar_.x      = stBodyParam.headToBackAxisLen;
    stLeftHeadPosByCar_.y      = 0.5 * stBodyParam.bodyWidth;
    stRightHeadPosByCar_.x     = stBodyParam.headToBackAxisLen;
    stRightHeadPosByCar_.y     = -0.5 * stBodyParam.bodyWidth;
    stLeftTailPosByCar_.x      = -1.0 * stBodyParam.tailToBackAxisLen;
    stLeftTailPosByCar_.y      = 0.5 * stBodyParam.bodyWidth;
    stRightTailPosByCar_.x     = -1.0 * stBodyParam.tailToBackAxisLen;
    stRightTailPosByCar_.y     = -0.5 * stBodyParam.bodyWidth;
    stLeftBackAxisPosByCar_.x  = 0.0;
    stLeftBackAxisPosByCar_.y  = 0.5 * stBodyParam.bodyWidth;
    stRightBackAxisPosByCar_.x = 0.0;
    stRightBackAxisPosByCar_.y = -0.5 * stBodyParam.bodyWidth;

    float minTurnRadius = GetTurnRadius(stBodyParam.maxSteeringAngleAbs);
    stMaxAngleLeftTurnCenterPosByCar_.x  = 0.0;
    stMaxAngleLeftTurnCenterPosByCar_.y  = minTurnRadius;
    stMaxAngleRightTurnCenterPosByCar_.x = 0.0;
    stMaxAngleRightTurnCenterPosByCar_.y = -1.0 * minTurnRadius;

    return 0;
}

float CAR_BODY_C::GetWheelBaseLen(void) const
{
    return stBodyParam_.wheelBaseLen;
}

void DYNC_CAR_C::SetCarPose(const COOR_2D_POSE_STRU &stCarPoseByMap)
{
    stCarPoseByMap_ = stCarPoseByMap;

    objMapToCarTf_.SetTransRelationship(stCarPoseByMap_);

    objMapToCarTf_.TransSonPoseToFather(stFrontWheelPoseByCar_, stCarFrontWheelPoseByMap_);
}

void DYNC_CAR_C::SetFrontWheelPose(const COOR_2D_POSE_STRU &stPoseByMap)
{
    stCarFrontWheelPoseByMap_ = stPoseByMap;
    
    objMapToCarTf_.SetTransRelationship(stCarFrontWheelPoseByMap_, stFrontWheelPoseByCar_);
    
    objMapToCarTf_.GetSonOriginPoseByFather(stCarPoseByMap_);
}

const COORDINATE_2D_STRU &DYNC_CAR_C::GetCarPos(void) const
{
    return stCarPoseByMap_.stPos;
}

void DYNC_CAR_C::GetCarPose(COOR_2D_POSE_STRU &stCarPoseByMap) const
{
    stCarPoseByMap = stCarPoseByMap_;
}

const COOR_2D_POSE_STRU &DYNC_CAR_C::GetCarPose(void) const
{
    return stCarPoseByMap_;
}

const COORDINATE_2D_STRU &DYNC_CAR_C::GetFrontWheelPos(void) const
{
    return stCarFrontWheelPoseByMap_.stPos;
}

void DYNC_CAR_C::GetFrontWheelPose(COOR_2D_POSE_STRU &stPoseByMap) const
{
    stPoseByMap = stCarFrontWheelPoseByMap_;
}

const COOR_2D_POSE_STRU &DYNC_CAR_C::GetFrontWheelPose(void) const
{
    return stCarFrontWheelPoseByMap_;
}

void DYNC_CAR_C::Get4CornerPosByMap(COORDINATE_2D_STRU &stLeftHeadPosByMap, COORDINATE_2D_STRU &stRightHeadPosByMap, COORDINATE_2D_STRU &stLeftTailPosByMap, COORDINATE_2D_STRU &stRightTailPosByMap, float sideProtectLen, float faceProectLen) const
{
    COORDINATE_2D_STRU stPosByCar;
    
    GetLeftHeadPosByCarWithProtect(faceProectLen, sideProtectLen, stPosByCar);
    TransformPosByCarToByMap(stPosByCar, stLeftHeadPosByMap);

    GetRightHeadPosByCarWithProtect(faceProectLen, sideProtectLen, stPosByCar);
    TransformPosByCarToByMap(stPosByCar, stRightHeadPosByMap);

    GetLeftTailPosByCarWithProtect(faceProectLen, sideProtectLen, stPosByCar);
    TransformPosByCarToByMap(stPosByCar, stLeftTailPosByMap);

    GetRightTailPosByCarWithProtect(faceProectLen, sideProtectLen, stPosByCar);
    TransformPosByCarToByMap(stPosByCar, stRightTailPosByMap);
}

void DYNC_CAR_C::TransformPosByMapToByCar(const COORDINATE_2D_STRU &stPosByMap, COORDINATE_2D_STRU &stPosByCar) const
{
    objMapToCarTf_.TransFatherPosToSon(stPosByMap, stPosByCar);
}

void DYNC_CAR_C::TransformPosByCarToByMap(const COORDINATE_2D_STRU &stPosByCar, COORDINATE_2D_STRU &stPosByMap) const
{
    objMapToCarTf_.TransSonPosToFather(stPosByCar, stPosByMap);
}

float MOVE_CAR_C::Move(float steeringAngle, float linearVel)
{
    const COOR_2D_POSE_STRU &stPreCarPose = GetCarPose();
    
    float moveLen       = linearVel * timeLenPerMove_;
    float deltaDirAgnle = moveLen * tan(steeringAngle) * inverseOfWheelBaseLen_;
    float moveDir       = stPreCarPose.dirAngle + 0.5 * deltaDirAgnle;

    stMoveInfo_.steeringAngle = steeringAngle;
    stMoveInfo_.moveLinearVel = linearVel;
    stMoveInfo_.dirAngularVel = deltaDirAgnle * inverseOfTimeLenPerMove_;

    COOR_2D_POSE_STRU stNewCarPose;
    stNewCarPose.stPos.x  = stPreCarPose.stPos.x + cos(moveDir) * moveLen;
    stNewCarPose.stPos.y  = stPreCarPose.stPos.y + sin(moveDir) * moveLen;
    stNewCarPose.dirAngle = AdjustAngle(stPreCarPose.dirAngle + deltaDirAgnle);
    SetCarPose(stNewCarPose);

    return ABS(moveLen);
}

float MOVE_CAR_C::MoveWithAccLimit(float steeringAngle, float linearVel)
{
    float linearVelWithAccLimit = GetLinearVelWithAccLimit(linearVel);

    return Move(steeringAngle, linearVelWithAccLimit);
}

void MOVE_CAR_C::ResetMoveInfo(void)
{
    stMoveInfo_.steeringAngle = 0.0;
    stMoveInfo_.moveLinearVel = 0.0;
    stMoveInfo_.dirAngularVel = 0.0;
}

void MOVE_CAR_C::GetMoveInfo(COOR_2D_POSE_STRU &stCarPoseByMap, CAR_MOVE_INFO_STRU &stMoveInfo) const
{
    stCarPoseByMap = GetCarPose();
    stMoveInfo     = stMoveInfo_;
}

float MOVE_CAR_C::GetLinearVelWithAccLimit(float linearVelCmd) const
{
    if ((stMoveInfo_.moveLinearVel * linearVelCmd) < 0.0) {
        /* 速度反向，先刹车减速到0.0 */
        return 0.0;
        
    } else if (IS_FLOAT_ALMOST_EQUAL(stMoveInfo_.moveLinearVel, linearVelCmd)) {
        /* 速度几乎不变，直接返回新速度 */
        return linearVelCmd;
        
    } else if ((stMoveInfo_.moveLinearVel >= 0) && (linearVelCmd > stMoveInfo_.moveLinearVel)) {
        /* 前向加速，按照最大加速度加速 */
        float velWithMaxAcc = stMoveInfo_.moveLinearVel + maxVelDiffOfPerPeriodAbs_;
        return MIN(velWithMaxAcc, linearVelCmd);
        
    } else if ((stMoveInfo_.moveLinearVel > 0) && (linearVelCmd < stMoveInfo_.moveLinearVel)) {
        /* 前向减速，不受减速度限制，直接返回新速度 */
        return linearVelCmd;
        
    } else if ((stMoveInfo_.moveLinearVel <= 0) && (linearVelCmd < stMoveInfo_.moveLinearVel)) {
        /* 后向加速，按照最大加速度加速 */
        float velWithMaxAcc = stMoveInfo_.moveLinearVel - maxVelDiffOfPerPeriodAbs_;
        return MAX(velWithMaxAcc, linearVelCmd);
        
    } else if ((stMoveInfo_.moveLinearVel < 0) && (linearVelCmd > stMoveInfo_.moveLinearVel)) {
        /* 后向减速，不受减速度限制，直接返回新速度 */
        return linearVelCmd;
        
    } else {
        ST_LOG_ERR("Asset!!!! preVel = %f, curVel = %f.", stMoveInfo_.moveLinearVel, linearVelCmd);
        return 0.0;
    }
}

/**************************************************************************************
功能描述: 设置行进方向
修改记录:
**************************************************************************************/
void CAR_DRIVER_C::SetMoveDir(BOOL isGoForward, BOOL isUseFrontWheelToGoForwardTrc)
{
    isGoForward_                   = isGoForward;
    isUseFrontWheelToGoForwardTrc_ = isUseFrontWheelToGoForwardTrc;
}

BOOL CAR_DRIVER_C::IsGoForward(void) const
{
    return isGoForward_;
}

/**************************************************************************************
功能描述: 设置车辆与路径对应的位姿
修改记录:
**************************************************************************************/
void CAR_DRIVER_C::SetRoutePose(const COOR_2D_POSE_STRU &stCarRoutePose)
{
    if (!isGoForward_) {
        /* 后退时，用户路径处理的位置为后轮中心，方向和车向相反。 */
        objCar_.SetCarPose(COOR_2D_POSE_STRU(stCarRoutePose.stPos.x, stCarRoutePose.stPos.y, AdjustAngle(stCarRoutePose.dirAngle + PI)));
        
    } else if (isUseFrontWheelToGoForwardTrc_) {
        objCar_.SetFrontWheelPose(stCarRoutePose);
        
    } else {
        objCar_.SetCarPose(stCarRoutePose);
    }
}

void CAR_DRIVER_C::SetRoutePose(const POSE_2D_C &objRoutePose)
{
    if (!isGoForward_) {
        /* 后退时，用户路径处理的位置为后轮中心，方向和车向相反。 */
        objCar_.SetCarPose(COOR_2D_POSE_STRU(objRoutePose.stPos.x, objRoutePose.stPos.y, AdjustAngle(objRoutePose.objDir.Val() + PI)));
        
    } else if (isUseFrontWheelToGoForwardTrc_) {
        objCar_.SetFrontWheelPose(objRoutePose.ToOld());
        
    } else {
        objCar_.SetCarPose(objRoutePose.ToOld());
    }
}

/**************************************************************************************
功能描述: 获取车辆与路径对应的位姿
修改记录:
**************************************************************************************/
void CAR_DRIVER_C::GetRoutePose(COOR_2D_POSE_STRU &stCarRoutePose) const
{
    if (!isGoForward_) {
        /* 后退时，用户路径处理的位置为后轮中心，方向和车向相反。 */
        objCar_.GetCarPose(stCarRoutePose);
        stCarRoutePose.dirAngle = AdjustAngle(stCarRoutePose.dirAngle + PI);
        
    } else if (isUseFrontWheelToGoForwardTrc_) {
        objCar_.GetFrontWheelPose(stCarRoutePose);
        
    } else {
        objCar_.GetCarPose(stCarRoutePose);
    }
}

void CAR_DRIVER_C::GetRoutePose(POSE_2D_C &objRoutePose) const
{
    if (!isGoForward_) {
        /* 后退时，用户路径处理的位置为后轮中心，方向和车向相反。 */
        objRoutePose = objCar_.GetCarPose();
        objRoutePose.objDir.Add(PI);
        
    } else if (isUseFrontWheelToGoForwardTrc_) {
        objRoutePose = objCar_.GetFrontWheelPose();
        
    } else {
        objRoutePose = objCar_.GetCarPose();
    }
}

/**************************************************************************************
功能描述: 获取跟踪目标的转向角(使用前轮跟踪)
修改记录:
**************************************************************************************/
float CAR_DRIVER_C::GetSteeringAngleOfFrontWheelTrc(const COORDINATE_2D_STRU &stAimPosByMap) const
{
    const COOR_2D_POSE_STRU &stCarFrontWheelPoseByMap = objCar_.GetFrontWheelPose();
    float                    maxSteeringAngleAbs      = objCar_.GetMaxSteeringAngleAbs();
    
    float steeringAngle = AdjustAngle(Calc2PointLineAngle(&stCarFrontWheelPoseByMap.stPos, &stAimPosByMap) - stCarFrontWheelPoseByMap.dirAngle);
    return LIMIT(steeringAngle, -1.0 * maxSteeringAngleAbs, maxSteeringAngleAbs);
}

/**************************************************************************************
功能描述: 获取后退时跟踪目标的转向角
修改记录:
**************************************************************************************/
float CAR_DRIVER_C::GetSteeringAngleOfBackWheelTrc(const COORDINATE_2D_STRU &stAimPosByMap) const
{
    const COOR_2D_POSE_STRU &stCarPose           = objCar_.GetCarPose();
    float                    maxSteeringAngleAbs = objCar_.GetMaxSteeringAngleAbs();
    
    /* 假设打转方向盘后，后轮经过一个圆弧到达目标点，则该圆弧的圆心点必在后轮轴上 */
    float backAxisLeftDirUnitVectorX = cos(stCarPose.dirAngle + 0.5 * PI);
    float backAxisLeftDirUnitVectorY = sin(stCarPose.dirAngle + 0.5 * PI);
    float wheelToAimVectorX = stAimPosByMap.x - stCarPose.stPos.x;
    float wheelToAimVectorY = stAimPosByMap.y - stCarPose.stPos.y;

    /* 上述两向量点乘，如果结果为0，表示目标和后轮在一条直线上直接后退即可 */
    float vectorDotProduct = (wheelToAimVectorX * backAxisLeftDirUnitVectorX + wheelToAimVectorY * backAxisLeftDirUnitVectorY);
    if (ABS(vectorDotProduct) < 1.0e-20) {
        return 0.0;
    }

    /* 计算后轮转向半径，大于0表示圆心在车辆左侧，小于0表示圆心在右侧 */
    float turnRadius = 0.5 * (wheelToAimVectorX * wheelToAimVectorX + wheelToAimVectorY * wheelToAimVectorY) / vectorDotProduct;

    /* 根据后轮转弯半径，计算前轮转向角 */
    float steeringAngle = objCar_.GetSteeringAngle(turnRadius);
    return LIMIT(steeringAngle, -1.0 * maxSteeringAngleAbs, maxSteeringAngleAbs);
}

/**************************************************************************************
功能描述: 获取跟踪目标的转向角
修改记录:
**************************************************************************************/
float CAR_DRIVER_C::GetAimPointSteeringAngle(const COORDINATE_2D_STRU &stAimPosByMap) const
{
    if (isGoForward_ && isUseFrontWheelToGoForwardTrc_) {
        return GetSteeringAngleOfFrontWheelTrc(stAimPosByMap);
    } else {
        return GetSteeringAngleOfBackWheelTrc(stAimPosByMap);
    }
}

float CAR_DRIVER_C::GetSimulationVel(void) const
{
    if (isGoForward_) {
        return objCar_.GetMaxMoveVelAbs();
    } else {
        return objCar_.GetMaxMoveVelAbs() * -1.0;
    }
}

float CAR_DRIVER_C::GetMaxMoveVel(void) const
{
    if (isGoForward_) {
        return objCar_.GetMaxMoveVelAbs();
    } else {
        return objCar_.GetMaxMoveVelAbs() * -0.5;
    }
}

#endif


